/*

Read data from the 9DOF Razor IMU board via arduino duemilanove board.

1. Upload this Sketch to the Arduino board
2. Disconnect the USB cable from the PC. 
3. Connect the jumper wires between the two boards
   according to the following table:

9DOF Razor IMU | Arduino Duemilanove
---------------|--------------------
TX0............|TX1.................
RX1............|RX0.................
3.3V...........|3V3.................
GND............|GND.................
---------------|--------------------

4. Connect again the USB Cable to the PC.
5. Open Serial Monitor.
6. Send the character '1'.
7. Send the character '0' to view the 9DOF Razor IMU Menu.
*/

/*
    Temperature output: rear-most sensor to front-most
                        for the motor controller electronics tube
                pins:   A10, A9, A8, A5
*/

#define NUM_TEMPS 4

int mcVoltPin = 4;
int tempPins[] { 10, 9, 8, 5 };
float temps[NUM_TEMPS];
unsigned long lastMeasureMs;

void setup() {
  Serial.begin(57600);
  Serial2.begin(9600);

  delay(1000);

  Serial2.print('3');

  Serial.println("imu");
  
  delay(1000);
  
  lastMeasureMs = millis();
}

void loop() {
  if(Serial2.available()) {
    char c = Serial2.read();
    Serial.write(c);

    if (c == '\n')
      tryOutputTemps();
  }
}

void tryOutputTemps() {
  unsigned long now = millis();
    
  if ((now - lastMeasureMs) > 1000) {
    for (int i = 0; i < NUM_TEMPS; i++)
      temps[i] = calcTemp(analogRead(tempPins[i]));

    float mcVoltage = calcVolts(analogRead(mcVoltPin));
    
    Serial.print("s");
    Serial.print(mcVoltage);
    Serial.print(";");
    
    for (int i = 0; i < NUM_TEMPS; i++) {
      Serial.print(temps[i]);
      Serial.print(";");
    }
      
    Serial.println();
    
    lastMeasureMs = now;
  }
}

float convAnalogToMilliVolts(int analog) {
  return analog * 5000.0 / 1023.0;
}

// analog input from MC volt meter
float calcVolts(int analog) {
  //return convAnalogToMilliVolts(analog) / 1000.0 * 13.592 - 37.333;
  return convAnalogToMilliVolts(analog) / 1000.0 * 12.887 - 35.969;
}

// analog input from temperature sensor
float calcTemp(int analog) {  
  return (convAnalogToMilliVolts(analog) * 33.0 / 380.0) - 48.68;
}

